#include "ieee_1547.h"
#include <unistd.h>
#include <sys/time.h>
#include "debug.h"
#include "msg_queue_interface.h"
#include "msg_queue_pid_define.h"
#include "function_interface.h"
#include <pthread.h>
#include <sys/syscall.h>
#include "misc.h"
#include <math.h>
#include "rated_limit.h"
#include "Commom/timer_interface.h"
#include "syslog.h"
#include "fpga_interface.h"
#include "ratio_coef.h"
#include "utils.h"


typedef struct
{
    float pwrFct;   /* 功率因数 */ 
    float actPwr;   /* 有功设定值 */
} modeCPF_t;


typedef struct
{
    float volRef;           /* 电压参考     */
    float v[VQ_POINT_QTY];  /* 电压控制点 */
    float q[VQ_POINT_QTY];  /* 无功控制点 */
    float tResp;            /* 响应时间     */
} modeVQ_t;


typedef struct
{
    float p[PQ_POINT_QTY];  /* 有功控制点 */
    float q[PQ_POINT_QTY];  /* 无功控制点 */
} modePQ_t;


typedef struct
{
    float reactPwr;         /* 无功设定 */
} modeCQ_t;
    

typedef struct
{
    float v[VP_POINT_QTY];  /* 电压控制点 */
    float p[VP_POINT_QTY];  /* 有功控制点 */
    float tResp;            /* 响应时间     */
    float absorbPwr;        /* 吸收功率设定 */
} modeVP_t;


typedef struct
{
    float dbUf;     /* 欠频死区 */
    float pAvl;     /* 可提供功率 */
    float kUf;      /* 欠频调频系数 */   
    float dbOf;     /* 过频死区 */
    float pMin;     /* 最小可吸收功率 */
    float kOf;      /* 过频调频系数 */      
    float tResp;    /* 响应时间     */
    float actPwr;   /* 有功设定值 */
} modeFP_t;


typedef struct
{
    float pMax;     /* 最大有功限制 */
} modePL_t;


typedef struct
{
    float qSet;         /* 无功输出值 */
    float pSet;         /* 有功输出值 */
    float qSetLast;     /* 上次无功输出值 */
    float pSetLast;     /* 上次有功输出值 */
} ieee1547Output_t;


typedef struct
{
    modeCPF_t modeCPF;  /* 恒功因 */
    modeVQ_t modeVQ;    /* 电压无功 */
    modePQ_t modePQ;    /* 有功无功 */
    modeCQ_t modeCQ;    /* 恒无功 */
    modeVP_t modeVP;    /* 电压有功 */
    modeFP_t modeFP;    /* 频率有功 */
    modePL_t modePL;    /* 功率限制 */
    ieee1547Output_t output;
} ieee1547_t;


typedef struct
{
    mode_t mode;                /* 模式类型 */
    modeSwitch_t modeSwitch;    /* 模式开关 */
    void (*pFuncHandle)(void);  /* 处理回调 */
} modeHandleTable_t;


static ieee1547_t ieee1547;


/**
 * @brief 恒功因模式-功因设定
 */
int Ieee1547_ModeCPF_SetPf(int16_t pfSet)
{
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(pfSet < -100 || pfSet > 100)
    {
        PRINT_WARN("pfSet(%d)", pfSet);
        return -1;
    }
#endif    
    
    ieee1547.modeCPF.pwrFct = (float)pfSet / 100;
    return 0;
}


/**
 * @brief 恒功因模式-有功设定
 */
void Ieee1547_ModeCPF_SetActPwr(int16_t actPwrSet)
{
    ieee1547.modeCPF.actPwr = (float)actPwrSet / 10;
}


/**
 * @brief 电压无功模式-电压参考设定
 */
int Ieee1547_ModeVQ_SetVolRef(uint16_t volRefSet)
{
    float volRated = 360.0F;
    funcInfGridState_t state;

#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(volRefSet < 95 || volRefSet > 105)
    {
        PRINT_WARN("volRefSet(%d)", volRefSet);
        return -1;
    }
#endif

    if(FuncInf_GetGridState(NULL, &state) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(state == FUNCINF_GRID_STATE_OFF)
    {
        if(FuncInf_GetAppointValue(FUNCINF_APPOINT_TYPE_AC_VOL, &volRated) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }
    }
    ieee1547.modeVQ.volRef = (float)volRefSet / 100 * volRated;

    return 0;
}


/**
 * @brief 电压无功模式-电压控制点设定
 */
int Ieee1547_ModeVQ_SetVol(uint16_t volSet, modeVQ_Point_t modeVQ_Point)
{
    if(modeVQ_Point >= VQ_POINT_QTY)
    {
        PRINT_WARN("modeVQ_Point(%d)", modeVQ_Point);
        return -1;
    }

#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float volRated = 360.0F;
    funcInfGridState_t state;

    if(FuncInf_GetGridState(NULL, &state) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(state == FUNCINF_GRID_STATE_OFF)
    {
        if(FuncInf_GetAppointValue(FUNCINF_APPOINT_TYPE_AC_VOL, &volRated) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }
    }
#endif

    switch(modeVQ_Point)
    {
        case VQ_POINT1:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(volSet < (uint16_t)(ieee1547.modeVQ.volRef - 0.18F * volRated) * 10 || volSet > (uint16_t)(ieee1547.modeVQ.v[VQ_POINT2] - 0.02F * volRated) * 10)
            {
                PRINT_WARN("volSet(%d)", volSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.v[modeVQ_Point] = (float)volSet / 10;
            break;
        
        case VQ_POINT2:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(volSet < (uint16_t)(ieee1547.modeVQ.volRef - 0.03F * volRated) * 10 || volSet > (uint16_t)ieee1547.modeVQ.volRef * 10)
            {
                PRINT_WARN("volSet(%d)", volSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.v[modeVQ_Point] = (float)volSet / 10;
            break;
        
        case VQ_POINT3:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(volSet < (uint16_t)ieee1547.modeVQ.volRef * 10 || volSet > (uint16_t)(ieee1547.modeVQ.volRef + 0.03F * volRated) * 10)
            {
                PRINT_WARN("volSet(%d)", volSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.v[modeVQ_Point] = (float)volSet / 10;
            break;
        
        case VQ_POINT4:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(volSet < (uint16_t)(ieee1547.modeVQ.v[VQ_POINT3] + 0.02F * volRated) * 10 || volSet > (uint16_t)(ieee1547.modeVQ.volRef + 0.18F * volRated) * 10)
            {
                PRINT_WARN("volSet(%d)", volSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.v[modeVQ_Point] = (float)volSet / 10;
            break;
        
        default:
            break;
    }

    return 0;
}


/**
 * @brief 电压无功模式-无功控制点设定
 */
int Ieee1547_ModeVQ_SetReactPwr(int16_t reactPwrSet, modeVQ_Point_t modeVQ_Point)
{
    if(modeVQ_Point >= VQ_POINT_QTY)
    {
        PRINT_WARN("modeVQ_Point(%d)", modeVQ_Point);
        return -1;
    }

#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
#endif   
    
    //ieee1547.modeVQ.q[modeVQ_Point] = (float)reactPwrSet / 10;

    switch(modeVQ_Point)
    {
        case VQ_POINT1:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(reactPwrSet < 0 * 10 || reactPwrSet > ratedPwr * 10)
            {
                PRINT_WARN("reactPwrSet(%d)", reactPwrSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.q[modeVQ_Point] = (float)reactPwrSet / 10;
            break;
        
        case VQ_POINT2:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(reactPwrSet < -1 * ratedPwr * 10 || reactPwrSet > ratedPwr * 10)
            {
                PRINT_WARN("reactPwrSet(%d)", reactPwrSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.q[modeVQ_Point] = (float)reactPwrSet / 10;
            break;
        
        case VQ_POINT3:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(reactPwrSet < -1 * ratedPwr * 10 || reactPwrSet > ratedPwr * 10)
            {
                PRINT_WARN("reactPwrSet(%d)", reactPwrSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.q[modeVQ_Point] = (float)reactPwrSet / 10;
            break;
        
        case VQ_POINT4:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(reactPwrSet < -1 * ratedPwr * 10 || reactPwrSet > 0 * 10)
            {
                PRINT_WARN("reactPwrSet(%d)", reactPwrSet);
                return -1;
            }
        #endif
            ieee1547.modeVQ.q[modeVQ_Point] = (float)reactPwrSet / 10;
            break;
        
        default:
            break;
    }

    return 0;
}


/**
 * @brief 电压无功模式-响应时间设定
 */
int Ieee1547_ModeVQ_SetRespTime(uint16_t respTimeSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(respTimeSet < 1 * 10 || respTimeSet > 90 * 10)
    {
        PRINT_WARN("respTimeSet(%d)", respTimeSet);
        return -1;
    }
#endif
    
    ieee1547.modeVQ.tResp = (float)respTimeSet / 10;
    return 0;
}


/**
 * @brief 有功无功模式-有功控制点设定
 */
int Ieee1547_ModePQ_SetActPwr(int16_t actPwrSet, modePQ_Point_t modePQ_Point)
{
    if(modePQ_Point >= PQ_POINT_QTY)
    {
        PRINT_WARN("modePQ_Point(%d)", modePQ_Point);
        return -1;
    }

#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
#endif    

    switch(modePQ_Point)
    {
        case PQ_POINT1A:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < 0 * 10 || actPwrSet > (int16_t)(ieee1547.modePQ.p[PQ_POINT2A] - 0.1F * ratedPwr) * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modePQ.p[modePQ_Point] = (float)actPwrSet / 10;
            break;
        
        case PQ_POINT2A:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < (int16_t)(0.4F * ratedPwr) * 10 || actPwrSet > (int16_t)(0.8F * ratedPwr) * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modePQ.p[modePQ_Point] = (float)actPwrSet / 10;
            break;
        
        case PQ_POINT3A:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < (int16_t)(ieee1547.modePQ.p[PQ_POINT2A] + 0.1F * ratedPwr) * 10 || actPwrSet > ratedPwr * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modePQ.p[modePQ_Point] = (float)actPwrSet / 10;
            break;
        
        case PQ_POINT1B:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < (int16_t)(ieee1547.modePQ.p[PQ_POINT2B] + 0.1F * ratedPwr) * 10 || actPwrSet > 0 * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modePQ.p[modePQ_Point] = (float)actPwrSet / 10;
            break;
        
        case PQ_POINT2B:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < (int16_t)(-0.8F * ratedPwr) * 10 || actPwrSet > (int16_t)(-0.4F * ratedPwr) * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modePQ.p[modePQ_Point] = (float)actPwrSet / 10;
            break;
        
        case PQ_POINT3B:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < -1 * ratedPwr * 10 || actPwrSet > (int16_t)(ieee1547.modePQ.p[PQ_POINT2B] - 0.1F * ratedPwr) * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modePQ.p[modePQ_Point] = (float)actPwrSet / 10;
            break;
        
        default:
            break;
    }

    return 0;
}


/**
 * @brief 有功无功模式-无功控制点设定
 */
int Ieee1547_ModePQ_SetReactPwr(int16_t reactPwrSet, modePQ_Point_t modePQ_Point)
{
    if(modePQ_Point >= PQ_POINT_QTY)
    {
        PRINT_WARN("modePQ_Point(%d)", modePQ_Point);
        return -1;
    }

#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(reactPwrSet < -1 * ratedPwr * 10 || reactPwrSet > ratedPwr * 10)
    {
        PRINT_WARN("reactPwrSet(%d)", reactPwrSet);
        return -1;
    }
#endif
    
    ieee1547.modePQ.q[modePQ_Point] = (float)reactPwrSet / 10;
    return 0;
}


/**
 * @brief 恒无功模式-无功设定
 */
void Ieee1547_ModeCQ_SetReactPwr(int16_t reactPwrSet)
{
    ieee1547.modeCQ.reactPwr = (float)reactPwrSet / 10;
}


/**
 * @brief 电压有功模式-电压控制点设定
 */
int Ieee1547_ModeVP_SetVol(uint16_t volSet, modeVP_Point_t modeVP_Point)
{
    if(modeVP_Point >= VP_POINT_QTY)
    {
        PRINT_WARN("modeVP_Point(%d)", modeVP_Point);
        return -1;
    }

#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float volRated = 360.0F;
    funcInfGridState_t state;

    if(FuncInf_GetGridState(NULL, &state) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(state == FUNCINF_GRID_STATE_OFF)
    {
        if(FuncInf_GetAppointValue(FUNCINF_APPOINT_TYPE_AC_VOL, &volRated) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }
    }
#endif

    switch(modeVP_Point)
    {
        case VP_POINT1:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(volSet < (uint16_t)(1.05F * volRated) * 10 || volSet > (uint16_t)(1.09F * volRated) * 10)
            {
                PRINT_WARN("volSet(%d)", volSet);
                return -1;
            }
        #endif
            ieee1547.modeVP.v[modeVP_Point] = (float)volSet / 10;
            break;
        
        case VP_POINT2:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(volSet < (uint16_t)(ieee1547.modeVP.v[VP_POINT1] + 0.01F * volRated) * 10 || volSet > (uint16_t)(1.1F * volRated) * 10)
            {
                PRINT_WARN("volSet(%d)", volSet);
                return -1;
            }
        #endif
            ieee1547.modeVP.v[modeVP_Point] = (float)volSet / 10;
            break;
        
        default:
            break;
    }

    return 0;
}


/**
 * @brief 电压有功模式-有功控制点设定
 */
int Ieee1547_ModeVP_SetActPwr(int16_t actPwrSet, modeVP_Point_t modeVP_Point)
{
    if(modeVP_Point >= VP_POINT_QTY)
    {
        PRINT_WARN("modeVP_Point(%d)", modeVP_Point);
        return -1;
    }
    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
#endif        
    //ieee1547.modeVP.p[modeVP_Point] = (float)actPwrSet / 10;

    switch(modeVP_Point)
    {
        case VP_POINT1:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet != ratedPwr * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modeVP.p[modeVP_Point] = (float)actPwrSet / 10;
            break;
        
        case VP_POINT2:
        #if(IEEE_1547_INPUT_RANGE_CHECK != 0)
            if(actPwrSet < -1 * ratedPwr * 10 || actPwrSet > ratedPwr * 10)
            {
                PRINT_WARN("actPwrSet(%d)", actPwrSet);
                return -1;
            }
        #endif
            ieee1547.modeVP.p[modeVP_Point] = (float)actPwrSet / 10;
            break;
        
        default:
            break;
    }

    return 0;
}


/**
 * @brief 电压有功模式-响应时间设定
 */
int Ieee1547_ModeVP_SetRespTime(uint16_t respTimeSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(respTimeSet < 0.5 * 10 || respTimeSet > 60 * 10)
    {
        PRINT_WARN("respTimeSet(%d)", respTimeSet);
        return -1;
    }
#endif
    
    ieee1547.modeVP.tResp = (float)respTimeSet / 10;
    return 0;
}


/**
 * @brief 电压有功模式-吸收功率设定
 */
void Ieee1547_ModeVP_SetAbsorbActPwr(int16_t absorbPwrSet)
{    
    ieee1547.modeVP.absorbPwr = (float)absorbPwrSet / 10;
}


/**
 * @brief 频率有功模式-欠频死区设定
 */
int Ieee1547_ModeFP_SetDbUf(uint16_t dbUfSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(dbUfSet < 17 || dbUfSet > 1000)
    {
        PRINT_WARN("dbUfSet(%d)", dbUfSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.dbUf = (float)dbUfSet / 1000;
    return 0;
}


/**
 * @brief 频率有功模式-可提供功率设定
 */
int Ieee1547_ModeFP_SetPavl(uint16_t pAvlSet)
{
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(pAvlSet != ratedPwr * 10)
    {
        PRINT_WARN("pAvlSet(%d)", pAvlSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.pAvl = (float)pAvlSet / 10;
    return 0;
}


/**
 * @brief 频率有功模式-欠频调频系数设定
 */
int Ieee1547_ModeFP_SetKuf(uint16_t kUfSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(kUfSet < 30 || kUfSet > 50)
    {
        PRINT_WARN("kUfSet(%d)", kUfSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.kUf = (float)kUfSet / 1000;
    return 0;
}

/**
 * @brief 频率有功模式-过频死区设定
 */
int Ieee1547_ModeFP_SetDbOf(uint16_t dbOfSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(dbOfSet < 17 || dbOfSet > 1000)
    {
        PRINT_WARN("dbOfSet(%d)", dbOfSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.dbOf = (float)dbOfSet / 1000;
    return 0;
}


/**
 * @brief 频率有功模式-最小可吸收功率设定
 */
int Ieee1547_ModeFP_SetPmin(int16_t pMinSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(pMinSet != -1 * ratedPwr * 10)
    {
        PRINT_WARN("pMinSet(%d)", pMinSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.pMin = (float)pMinSet / 10;
    return 0;
}


/**
 * @brief 频率有功模式-过频调频系数设定
 */
int Ieee1547_ModeFP_SetKof(uint16_t kOfSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(kOfSet < 30 || kOfSet > 50)
    {
        PRINT_WARN("kOfSet(%d)", kOfSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.kOf = (float)kOfSet / 1000;
    return 0;
}


/**
 * @brief 频率有功模式-响应时间设定
 */
int Ieee1547_ModeFP_SetRespTime(uint16_t respTimeSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    if(respTimeSet < 1 * 10 || respTimeSet > 10 * 10)
    {
        PRINT_WARN("respTimeSet(%d)", respTimeSet);
        return -1;
    }
#endif
    
    ieee1547.modeFP.tResp = (float)respTimeSet / 10;
    return 0;
}


/**
 * @brief 频率有功模式-扰动功率设定
 */
void Ieee1547_ModeFP_SetActPwr(int16_t actPwrSet)
{    
    ieee1547.modeFP.actPwr = (float)actPwrSet / 10;
}


/**
 * @brief 有功限制模式-最大功率限制设定
 */
int Ieee1547_ModePL_SetPmax(uint16_t pMaxSet)
{    
#if(IEEE_1547_INPUT_RANGE_CHECK != 0)
    float ratedPwr = 2000.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    if(pMaxSet > (uint16_t)(ratedPwr * 1.3F) * 10)
    {
        PRINT_WARN("pMaxSet(%d)", pMaxSet);
        return -1;
    }
#endif
    
    ieee1547.modePL.pMax = (float)pMaxSet / 10;
    return 0;
}


/**
 * @brief 恒功因模式-处理
 */
static void Ieee1547_ModeCPF_Handle(void)
{
    modeCPF_t *pModeCPF = &ieee1547.modeCPF;
    ieee1547Output_t *pOutput = &ieee1547.output;
    float pwrFct, actPwr, reactPwr;
    uint16_t fpgaVal;

    pwrFct = pModeCPF->pwrFct;
    actPwr = pModeCPF->actPwr;

    if(FltCmp_IsZero(pwrFct))
    {
        PRINT_ERROR("%.3f", pwrFct);
        return;
    }
    reactPwr = actPwr / pwrFct * sqrt(1.0F - pwrFct * pwrFct);
    
    //if(pOutput->qSet != reactPwr)
    {
        pOutput->qSet = reactPwr;
        RatioCoef_GetFpgaValue(FPGA_ADDR_APPOINT_TYPE_REACT_PWR, pOutput->qSet, &fpgaVal);
        FuncInf_DirectWriteFpga(FUNCINF_OPERATOR_1547, FPGA_ADDR_APPOINT_TYPE_REACT_PWR, fpgaVal);
        PRINT_DEBUG("%d", fpgaVal);
    }

    PRINT_DEBUG("%.1fkW - %.2f -> %.1fkVar", actPwr, pwrFct, reactPwr);
}


/**
 * @brief 电压无功模式-处理
 */
static void Ieee1547_ModeVQ_Handle(void)
{
    modeVQ_t *pModeVQ = &ieee1547.modeVQ;
    ieee1547Output_t *pOutput = &ieee1547.output;
    float volCurPhase[3], volCurAvr, volCur, reactPwr = 0, pwrDlt;
    float q1, q2, v1, v2;
    uint16_t fpgaVal;

#ifndef IEEE_1547_DEBUG_MODE
    if(FuncInf_GetVoltage(FUNCINF_VOL_TYPE_PHASE_A, &volCurPhase[0]) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FuncInf_GetVoltage(FUNCINF_VOL_TYPE_PHASE_B, &volCurPhase[1]) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FuncInf_GetVoltage(FUNCINF_VOL_TYPE_PHASE_C, &volCurPhase[2]) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FuncInf_GetDebugVal(FUNCINF_DBG_VAL_IDX_19, &volCur) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    volCurAvr = (volCurPhase[0] + volCurPhase[1] + volCurPhase[2]) / 3;
    volCur += volCurAvr;
#else
    FuncInf_GetDebugVal(FUNCINF_DBG_VAL_IDX_0, &volCur);
#endif

    if(FltCmp_IsALessEqualB(volCur, pModeVQ->v[VQ_POINT1]))
    {
        reactPwr = pModeVQ->q[VQ_POINT1];
    }
    else if(FltCmp_IsAGreaterThanB(volCur, pModeVQ->v[VQ_POINT1]) && FltCmp_IsALessThanB(volCur, pModeVQ->v[VQ_POINT2]))
    {
        q1 = pModeVQ->q[VQ_POINT1];
        v1 = pModeVQ->v[VQ_POINT1];
        q2 = pModeVQ->q[VQ_POINT2];
        v2 = pModeVQ->v[VQ_POINT2];
        if(FltCmp_IsZero(v2 - v1))
        {
            PRINT_ERROR("%.3f %.3f", v2, v1);
            return;
        }
        reactPwr = (q2 - q1) / (v2 - v1) * (volCur - v1) + q1;
    }
    else if(FltCmp_IsEqual(volCur, pModeVQ->v[VQ_POINT2]))
    {
        reactPwr = pModeVQ->q[VQ_POINT2];
    }
    else if(FltCmp_IsAGreaterThanB(volCur, pModeVQ->v[VQ_POINT2]) && FltCmp_IsALessThanB(volCur, pModeVQ->volRef))
    {
        q1 = pModeVQ->q[VQ_POINT2];
        v1 = pModeVQ->v[VQ_POINT2];
        q2 = 0;
        v2 = pModeVQ->volRef;
        if(FltCmp_IsZero(v2 - v1))
        {
            PRINT_ERROR("%.3f %.3f", v2, v1);
            return;
        }
        reactPwr = (q2 - q1) / (v2 - v1) * (volCur - v1) + q1;
    }
    else if(FltCmp_IsEqual(volCur, pModeVQ->volRef))
    {
        reactPwr = 0;
    }
    else if(FltCmp_IsAGreaterThanB(volCur, pModeVQ->volRef) && FltCmp_IsALessThanB(volCur, pModeVQ->v[VQ_POINT3]))
    {
        q1 = 0;
        v1 = pModeVQ->volRef;
        q2 = pModeVQ->q[VQ_POINT3];
        v2 = pModeVQ->v[VQ_POINT3];
        if(FltCmp_IsZero(v2 - v1))
        {
            PRINT_ERROR("%.3f %.3f", v2, v1);
            return;
        }
        reactPwr = (q2 - q1) / (v2 - v1) * (volCur - v1) + q1;
    }
    else if(FltCmp_IsEqual(volCur, pModeVQ->v[VQ_POINT3]))
    {
        reactPwr = pModeVQ->q[VQ_POINT3];
    }
    else if(FltCmp_IsAGreaterThanB(volCur, pModeVQ->v[VQ_POINT3]) && FltCmp_IsALessThanB(volCur, pModeVQ->v[VQ_POINT4]))
    {
        q1 = pModeVQ->q[VQ_POINT3];
        v1 = pModeVQ->v[VQ_POINT3];
        q2 = pModeVQ->q[VQ_POINT4];
        v2 = pModeVQ->v[VQ_POINT4];
        if(FltCmp_IsZero(v2 - v1))
        {
            PRINT_ERROR("%.3f %.3f", v2, v1);
            return;
        }
        reactPwr = (q2 - q1) / (v2 - v1) * (volCur - v1) + q1;
    }
    else if(FltCmp_IsAGreaterEqualB(volCur, pModeVQ->v[VQ_POINT4]))
    {
        reactPwr = pModeVQ->q[VQ_POINT4];
    }

    //if(pOutput->qSet != reactPwr)
    {
        if(FltCmp_IsZero(pModeVQ->tResp))
        {
            PRINT_ERROR("%.3f", pModeVQ->tResp);
            return;
        }
        pwrDlt = (reactPwr - pOutput->qSetLast) / (pModeVQ->tResp * 1000 / IEEE_1547_THREAD_POLLING_INTERVAL);
        pOutput->qSet += pwrDlt;
        PRINT_DEBUG("%.3fkW %.3fkW %.3fkW", pwrDlt, pOutput->qSet, reactPwr);
        if((FltCmp_IsAGreaterEqualB(pwrDlt, 0) && FltCmp_IsAGreaterEqualB(pOutput->qSet, reactPwr)) ||
           (FltCmp_IsALessThanB(pwrDlt, 0) && FltCmp_IsALessEqualB(pOutput->qSet, reactPwr)))
        {
            pOutput->qSet = reactPwr;
            pOutput->qSetLast = reactPwr;
        }
        RatioCoef_GetFpgaValue(FPGA_ADDR_APPOINT_TYPE_REACT_PWR, pOutput->qSet, &fpgaVal);
        FuncInf_DirectWriteFpga(FUNCINF_OPERATOR_1547, FPGA_ADDR_APPOINT_TYPE_REACT_PWR, fpgaVal);
        PRINT_DEBUG("%d", fpgaVal);
    }
    
    PRINT_DEBUG("%.1fV -> %.1fkVar", volCur, reactPwr);
}


/**
 * @brief 有功无功模式-处理
 */
static void Ieee1547_ModePQ_Handle(void)
{
    modePQ_t *pModePQ = &ieee1547.modePQ;
    ieee1547Output_t *pOutput = &ieee1547.output;
    float pacCur, reactPwr = 0;
    float q1, q2, p1, p2;
    uint16_t fpgaVal;

#ifndef IEEE_1547_DEBUG_MODE
    if(FuncInf_GetPower(FUNCINF_PWR_TYPE_ACT, &pacCur) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
#else
    FuncInf_GetDebugVal(FUNCINF_DBG_VAL_IDX_1, &pacCur);
#endif

    if(FltCmp_IsALessEqualB(pacCur, pModePQ->p[PQ_POINT3B]))
    {
        reactPwr = pModePQ->q[PQ_POINT3B];
    }
    else if(FltCmp_IsAGreaterThanB(pacCur, pModePQ->p[PQ_POINT3B]) && FltCmp_IsALessThanB(pacCur, pModePQ->p[PQ_POINT2B]))
    {
        q1 = pModePQ->q[PQ_POINT3B];
        p1 = pModePQ->p[PQ_POINT3B];
        q2 = pModePQ->q[PQ_POINT2B];
        p2 = pModePQ->p[PQ_POINT2B];
        if(FltCmp_IsZero(p2 - p1))
        {
            PRINT_ERROR("%.3f %.3f", p2, p1);
            return;
        }
        reactPwr = (q2 - q1) / (p2 - p1) * (pacCur - p1) + q1;
    }
    else if(FltCmp_IsEqual(pacCur, pModePQ->p[PQ_POINT2B]))
    {
        reactPwr = pModePQ->q[PQ_POINT2B];
    }
    else if(FltCmp_IsAGreaterThanB(pacCur, pModePQ->p[PQ_POINT2B]) && FltCmp_IsALessThanB(pacCur, pModePQ->p[PQ_POINT1B]))
    {
        q1 = pModePQ->q[PQ_POINT2B];
        p1 = pModePQ->p[PQ_POINT2B];
        q2 = pModePQ->q[PQ_POINT1B];
        p2 = pModePQ->p[PQ_POINT1B];
        if(FltCmp_IsZero(p2 - p1))
        {
            PRINT_ERROR("%.3f %.3f", p2, p1);
            return;
        }
        reactPwr = (q2 - q1) / (p2 - p1) * (pacCur - p1) + q1;
    }
    else if(FltCmp_IsEqual(pacCur, pModePQ->p[PQ_POINT1B]))
    {
        reactPwr = pModePQ->q[PQ_POINT1B];
    }
    else if(FltCmp_IsAGreaterThanB(pacCur, pModePQ->p[PQ_POINT1B]) && FltCmp_IsALessThanB(pacCur, 0.0F))
    {
        q1 = pModePQ->q[PQ_POINT1B];
        p1 = pModePQ->p[PQ_POINT1B];
        q2 = 0;
        p2 = 0;
        if(FltCmp_IsZero(p2 - p1))
        {
            PRINT_ERROR("%.3f %.3f", p2, p1);
            return;
        }
        reactPwr = (q2 - q1) / (p2 - p1) * (pacCur - p1) + q1;
    }
    else if(FltCmp_IsEqual(pacCur, 0))
    {
        reactPwr = 0;
    }
    else if(FltCmp_IsAGreaterThanB(pacCur, 0) && FltCmp_IsALessThanB(pacCur, pModePQ->p[PQ_POINT1A]))
    {
        q1 = 0;
        p1 = 0;
        q2 = pModePQ->q[PQ_POINT1A];
        p2 = pModePQ->p[PQ_POINT1A];
        if(FltCmp_IsZero(p2 - p1))
        {
            PRINT_ERROR("%.3f %.3f", p2, p1);
            return;
        }
        reactPwr = (q2 - q1) / (p2 - p1) * (pacCur - p1) + q1;
    }
    else if(FltCmp_IsEqual(pacCur, pModePQ->p[PQ_POINT1A]))
    {
        reactPwr = pModePQ->q[PQ_POINT1A];
    }
    else if(FltCmp_IsAGreaterThanB(pacCur, pModePQ->p[PQ_POINT1A]) && FltCmp_IsALessThanB(pacCur, pModePQ->p[PQ_POINT2A]))
    {
        q1 = pModePQ->q[PQ_POINT1A];
        p1 = pModePQ->p[PQ_POINT1A];
        q2 = pModePQ->q[PQ_POINT2A];
        p2 = pModePQ->p[PQ_POINT2A];
        if(FltCmp_IsZero(p2 - p1))
        {
            PRINT_ERROR("%.3f %.3f", p2, p1);
            return;
        }
        reactPwr = (q2 - q1) / (p2 - p1) * (pacCur - p1) + q1;
    }
    else if(FltCmp_IsEqual(pacCur, pModePQ->p[PQ_POINT2A]))
    {
        reactPwr = pModePQ->q[PQ_POINT2A];
    }
    else if(FltCmp_IsAGreaterThanB(pacCur, pModePQ->p[PQ_POINT2A]) && FltCmp_IsALessThanB(pacCur, pModePQ->p[PQ_POINT3A]))
    {
        q1 = pModePQ->q[PQ_POINT2A];
        p1 = pModePQ->p[PQ_POINT2A];
        q2 = pModePQ->q[PQ_POINT3A];
        p2 = pModePQ->p[PQ_POINT3A];
        if(FltCmp_IsZero(p2 - p1))
        {
            PRINT_ERROR("%.3f %.3f", p2, p1);
            return;
        }
        reactPwr = (q2 - q1) / (p2 - p1) * (pacCur - p1) + q1;
    }
    else if(FltCmp_IsAGreaterEqualB(pacCur, pModePQ->p[PQ_POINT3A]))
    {
        reactPwr = pModePQ->q[PQ_POINT3A];
    }

    //if(pOutput->qSet != reactPwr)
    {
        pOutput->qSet = reactPwr;
        RatioCoef_GetFpgaValue(FPGA_ADDR_APPOINT_TYPE_REACT_PWR, pOutput->qSet, &fpgaVal);
        FuncInf_DirectWriteFpga(FUNCINF_OPERATOR_1547, FPGA_ADDR_APPOINT_TYPE_REACT_PWR, fpgaVal);
        PRINT_DEBUG("%d", fpgaVal);
    }
    
    PRINT_DEBUG("%.1fkW -> %.1fkVar", pacCur, reactPwr);
}


/**
 * @brief 恒无功模式-处理
 */
static void Ieee1547_ModeCQ_Handle(void)
{
    modeCQ_t *pModeCQ = &ieee1547.modeCQ;
    ieee1547Output_t *pOutput = &ieee1547.output;
    float reactPwr;
    uint16_t fpgaVal;

    reactPwr = pModeCQ->reactPwr;
    //if(pOutput->qSet != reactPwr)
    {
        pOutput->qSet = reactPwr;
        RatioCoef_GetFpgaValue(FPGA_ADDR_APPOINT_TYPE_REACT_PWR, pOutput->qSet, &fpgaVal);
        FuncInf_DirectWriteFpga(FUNCINF_OPERATOR_1547, FPGA_ADDR_APPOINT_TYPE_REACT_PWR, fpgaVal);
        PRINT_DEBUG("%d", fpgaVal);
    }
    
    PRINT_DEBUG("-> %.1fkVar", reactPwr);
}


/**
 * @brief 电压有功模式-处理
 */
static void Ieee1547_ModeVP_Handle(void)
{
    modeVP_t *pModeVP = &ieee1547.modeVP;
    ieee1547Output_t *pOutput = &ieee1547.output;
    float actPwr = 0, volCurPhase[3], volCurAvr, volCur, pwrDlt;
    float p1, p2, v1, v2;
    uint16_t fpgaVal;

#ifndef IEEE_1547_DEBUG_MODE
    if(FuncInf_GetVoltage(FUNCINF_VOL_TYPE_PHASE_A, &volCurPhase[0]) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FuncInf_GetVoltage(FUNCINF_VOL_TYPE_PHASE_B, &volCurPhase[1]) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FuncInf_GetVoltage(FUNCINF_VOL_TYPE_PHASE_C, &volCurPhase[2]) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FuncInf_GetDebugVal(FUNCINF_DBG_VAL_IDX_19, &volCur) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    volCurAvr = (volCurPhase[0] + volCurPhase[1] + volCurPhase[2]) / 3;
    volCur += volCurAvr;
#else
    FuncInf_GetDebugVal(FUNCINF_DBG_VAL_IDX_0, &volCur);
#endif

    if(FltCmp_IsALessEqualB(volCur, pModeVP->v[VP_POINT1]))
    {
        actPwr = pModeVP->p[VP_POINT1];
    }
    else if(FltCmp_IsAGreaterThanB(volCur, pModeVP->v[VP_POINT1]) && FltCmp_IsALessThanB(volCur, pModeVP->v[VP_POINT2]))
    {
        p1 = pModeVP->p[VP_POINT1];
        v1 = pModeVP->v[VP_POINT1];
        p2 = pModeVP->p[VP_POINT2];
        v2 = pModeVP->v[VP_POINT2];
        if(FltCmp_IsZero(v2 - v1))
        {
            PRINT_ERROR("%.3f %.3f", v2, v1);
            return;
        }
        actPwr = (p2 - p1) / (v2 - v1) * (volCur - v1) + p1;
    }
    else if(FltCmp_IsAGreaterEqualB(volCur, pModeVP->v[VP_POINT2]))
    {
        actPwr = pModeVP->p[VP_POINT2];
    }

    if(FltCmp_IsAGreaterThanB(actPwr, 0.0F))
    {
        PRINT_DEBUG("ActPwr(%.1fkW)  AbsorbPwr(%.1fkW)", actPwr, pModeVP->absorbPwr);
        actPwr = pModeVP->absorbPwr > actPwr ? actPwr : pModeVP->absorbPwr;
    }

    //if(pOutput->pSet != actPwr)
    {
        if(FltCmp_IsZero(pModeVP->tResp))
        {
            PRINT_ERROR("%.3f", pModeVP->tResp);
            return;
        }
        pwrDlt = (actPwr - pOutput->pSetLast) / (pModeVP->tResp * 1000 / IEEE_1547_THREAD_POLLING_INTERVAL);
        pOutput->pSet += pwrDlt;
        PRINT_DEBUG("%.3fkW %.3fkW %.3fkW", pwrDlt, pOutput->pSet, actPwr);
        if((FltCmp_IsAGreaterEqualB(pwrDlt, 0) && FltCmp_IsAGreaterEqualB(pOutput->pSet, actPwr)) ||
           (FltCmp_IsALessThanB(pwrDlt, 0) && FltCmp_IsALessEqualB(pOutput->pSet, actPwr)))
        {
            pOutput->pSet = actPwr;
            pOutput->pSetLast = actPwr;
        }

        RatioCoef_GetFpgaValue(FPGA_ADDR_APPOINT_TYPE_ACT_PWR, pOutput->pSet, &fpgaVal);
        FuncInf_DirectWriteFpga(FUNCINF_OPERATOR_1547, FPGA_ADDR_APPOINT_TYPE_ACT_PWR, fpgaVal);
        PRINT_DEBUG("%d", fpgaVal);
    }
    
    PRINT_DEBUG("%.1fV -> %.1fkW", volCur, actPwr);
}


/**
 * @brief 频率有功模式-处理
 */
static void Ieee1547_ModeFP_Handle(void)
{
    modeFP_t *pModeFP = &ieee1547.modeFP;
    ieee1547Output_t *pOutput = &ieee1547.output;
    float actPwr, freqCur, pwrDlt, pwrTmp, ratedPwr = 2000.0F, freqRated;
    uint16_t fpgaVal;

#ifndef IEEE_1547_DEBUG_MODE
    if(FuncInf_GetFrequency(NULL, &freqCur) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
#else
    FuncInf_GetDebugVal(FUNCINF_DBG_VAL_IDX_2, &freqCur);
#endif

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }

    freqRated = FltCmp_IsALessThanB(freqCur, 55.0F) ? 50.0F : 60.0F;

    if(FltCmp_IsALessThanB(freqCur, freqRated - pModeFP->dbUf))
    {
        if(FltCmp_IsZero(pModeFP->kUf))
        {
            PRINT_ERROR("%.3f", pModeFP->kUf);
            return;
        }
        pwrTmp = pModeFP->actPwr + (freqRated - pModeFP->dbUf - freqCur) / freqRated / pModeFP->kUf * ratedPwr;
        actPwr = pwrTmp < pModeFP->pAvl ? pwrTmp : pModeFP->pAvl;
    }
    else if(FltCmp_IsAGreaterThanB(freqCur, freqRated + pModeFP->dbOf))
    {
        if(FltCmp_IsZero(pModeFP->kOf))
        {
            PRINT_ERROR("%.3f", pModeFP->kOf);
            return;
        }
        pwrTmp = pModeFP->actPwr - (freqCur - freqRated - pModeFP->dbOf) / freqRated / pModeFP->kOf * ratedPwr;
        actPwr = pwrTmp > pModeFP->pMin ? pwrTmp : pModeFP->pMin;
    }
    else
    {
        actPwr = pModeFP->actPwr;
    }

    //if(pOutput->pSet != actPwr)
    {
        if(FltCmp_IsZero(pModeFP->tResp))
        {
            PRINT_ERROR("%.3f", pModeFP->tResp);
            return;
        }
        pwrDlt = (actPwr - pOutput->pSetLast) / (pModeFP->tResp * 1000 / IEEE_1547_THREAD_POLLING_INTERVAL);
        pOutput->pSet += pwrDlt;
        PRINT_DEBUG("%.3fkW %.3fkW %.3fkW", pwrDlt, pOutput->pSet, actPwr);
        if((FltCmp_IsAGreaterEqualB(pwrDlt, 0) && FltCmp_IsAGreaterEqualB(pOutput->pSet, actPwr)) ||
           (FltCmp_IsALessThanB(pwrDlt, 0) && FltCmp_IsALessEqualB(pOutput->pSet, actPwr)))
        {
            pOutput->pSet = actPwr;
            pOutput->pSetLast = actPwr;
        }

        RatioCoef_GetFpgaValue(FPGA_ADDR_APPOINT_TYPE_ACT_PWR, pOutput->pSet, &fpgaVal);
        FuncInf_DirectWriteFpga(FUNCINF_OPERATOR_1547, FPGA_ADDR_APPOINT_TYPE_ACT_PWR, fpgaVal);
        PRINT_DEBUG("%d", fpgaVal);
    }
    
    PRINT_DEBUG("%.3fHz -> %.1fkW", freqCur, actPwr);
}


/**
 * @brief 有功限制模式-处理
 */
static void Ieee1547_ModePL_Handle(void)
{
    modePL_t *pModePL = &ieee1547.modePL;
    float ratedPwr = 2000.0F, percentage = 1.0F;

    if(FuncInf_GetRatedPwr(NULL, &ratedPwr) != FUNCINF_ERR_NONE)
    {
        PRINT_WARN();
    }
    if(FltCmp_IsZero(ratedPwr))
    {
        PRINT_ERROR("%.3f", ratedPwr);
        return;
    }
    percentage = pModePL->pMax / ratedPwr;
    if(RatedLimit_SetPercentage(RATED_LIMIT_OWNER_1547_P_LIMIT, percentage) != 0)
    {
        PRINT_WARN();
    }
    
    PRINT_DEBUG("%.1fkW", pModePL->pMax);
}


static modeHandleTable_t modeHandleTable[] = 
{
    {MODE_CPF, MODE_SWITCH_OFF, Ieee1547_ModeCPF_Handle}, /* 恒功因 */
    {MODE_VQ , MODE_SWITCH_OFF, Ieee1547_ModeVQ_Handle},  /* 电压无功 */
    {MODE_PQ , MODE_SWITCH_OFF, Ieee1547_ModePQ_Handle},  /* 有功无功 */
    {MODE_CQ , MODE_SWITCH_OFF, Ieee1547_ModeCQ_Handle},  /* 恒无功 */
    {MODE_VP , MODE_SWITCH_OFF, Ieee1547_ModeVP_Handle},  /* 电压有功 */
    {MODE_FP , MODE_SWITCH_OFF, Ieee1547_ModeFP_Handle},  /* 频率有功 */
    {MODE_PL , MODE_SWITCH_OFF, Ieee1547_ModePL_Handle},  /* 有功限制 */
};


/**
 * @brief 模式开关设定
 */
void Ieee1547_SetModeSwitch(mode1547_t mode, modeSwitch_t modeSwitch)
{
    uint16_t idx;
    
    if(mode >= MODE_QTY || modeSwitch >= MODE_SWITCH_QTY)
    {
        PRINT_WARN("mode(%d) modeSwitch(%d)", mode, modeSwitch);
        return;
    }

    for(idx = 0; idx < sizeof(modeHandleTable) / sizeof(modeHandleTable_t); idx++)
    {
        if(modeHandleTable[idx].mode == mode)
        {
            if(modeHandleTable[idx].modeSwitch != modeSwitch)
            {
                modeHandleTable[idx].modeSwitch = modeSwitch;
                usleep(IEEE_1547_THREAD_POLLING_INTERVAL * 1000);
                if(modeHandleTable[idx].mode == MODE_CPF || 
                   modeHandleTable[idx].mode == MODE_VQ || 
                   modeHandleTable[idx].mode == MODE_PQ || 
                   modeHandleTable[idx].mode == MODE_CQ)
                {
                    ieee1547.output.qSet = 0;
                    ieee1547.output.qSetLast = 0;
                    if(FuncInf_SetAppointValue(FUNCINF_OPERATOR_1547, FUNCINF_APPOINT_TYPE_REACT_PWR, 0) != FUNCINF_ERR_NONE)
                    {
                        PRINT_WARN();
                    }
                }
                else if(modeHandleTable[idx].mode == MODE_VP || 
                   modeHandleTable[idx].mode == MODE_FP)
                {
                    ieee1547.output.pSet = 0;
                    ieee1547.output.pSetLast = 0;
                    if(FuncInf_SetAppointValue(FUNCINF_OPERATOR_1547, FUNCINF_APPOINT_TYPE_ACT_PWR, 0) != FUNCINF_ERR_NONE)
                    {
                        PRINT_WARN();
                    }
                }
            }
            break;
        }
    }

    if(Ieee1547_GetModeSwitch(MODE_PL) == MODE_SWITCH_OFF)
    {
        if(RatedLimit_SetPercentage(RATED_LIMIT_OWNER_1547_P_LIMIT, 1.0F) != 0)
        {
            PRINT_WARN();
        }
    }
}


/**
 * @brief 模式开关获取
 */
modeSwitch_t Ieee1547_GetModeSwitch(mode1547_t mode)
{
    uint16_t idx;
    if(mode >= MODE_QTY)
    {
        PRINT_WARN("mode(%d)", mode);
        return MODE_SWITCH_OFF;
    }

    for(idx = 0; idx < sizeof(modeHandleTable) / sizeof(modeHandleTable_t); idx++)
    {
        if(modeHandleTable[idx].mode == mode)
        {
            return modeHandleTable[idx].modeSwitch;
        }
    }

    return MODE_SWITCH_OFF;
}


/**
 * @brief IEEE1547数据初始化
 */
static void Ieee1547_Init(void)
{
    float fltVal = 0.0F;
    funcInf1547ParamType_t type;

    for(type = 0; type <= FUNCINF_1547_PARAM_TYPE_PL_PMAX; type++)
    {
        if(FuncInf_Get1547Param(type, &fltVal) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }
        if(FuncInf_Set1547Param(FUNCINF_OPERATOR_INIT, type, fltVal) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }
    }

    Ieee1547_SendMessage();
}


static timer_t timerIdIeee1547 = NULL;


static void Ieee1547_HandleSignal(union sigval arg)
{
    ((void)(arg));
    // PRINT_DEBUG();

    uint16_t idx;
    
    for(idx = 0; idx < sizeof(modeHandleTable) / sizeof(modeHandleTable_t); idx++)
    {
        if(modeHandleTable[idx].modeSwitch == MODE_SWITCH_ON)
        {
            if(modeHandleTable[idx].pFuncHandle)
            {
                modeHandleTable[idx].pFuncHandle();
            }
        }
    }
}


static int Ieee1547_Run(time_t sec, long nsec)
{
    if(TimerInf_Create(&timerIdIeee1547, Ieee1547_HandleSignal, NULL) != 0)
    {
        PRINT_WARN();
        return -1;
    }

    if(TimerInf_Start(timerIdIeee1547, sec, nsec, TIMER_CYCLE) != 0)
    {
        PRINT_WARN();
        return -2;
    }
    
    syslog(LOG_INFO, "%s: IEEE1547 Run", __func__);
    return 0;
}


static int Ieee1547_Stop(void)
{
    if(TimerInf_Delete(&timerIdIeee1547) != 0)
    {
        PRINT_WARN();
        return -1;
    }

    if(RatedLimit_SetPercentage(RATED_LIMIT_OWNER_1547_P_LIMIT, 1.0F) != 0)
    {
        // PRINT_WARN();
        return -2;
    }

    // syslog(LOG_INFO, "%s: IEEE1547 Stop", __func__);
    return 0;
}


void Ieee1547_Thread(void *pParam)
{    
    Misc_InitThread(__func__);

    uint16_t testAlone = 0, strtg1547 = 0;
    int err, msgId;
    char msgData[MSG_DATA_LEN] = {0};

    if(MsgQueue_Get(MSG_QUEUE_PID_IEEE1547, &msgId) < 0)
	{
        // PRINT_ERROR("Get error");
		return;
	}

    Ieee1547_Init();

    while(1)
    {
        if((err = MsgQueue_Recv1(msgId, msgData)) < 0)
        {
            // PRINT_ERROR("Recv error[%d]", err);
        }
        // PRINT_DEBUG();
        
        if(FuncInf_GetTestSwitch(FUNCINF_TEST_SWITCH_TYPE_ALONE_TEST, &testAlone) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }
        if(FuncInf_GetStrategySwitch(FUNCINF_STRTG_SWITCH_TYPE_1547, &strtg1547) != FUNCINF_ERR_NONE)
        {
            PRINT_WARN();
        }

        /* 1547策略开启并且单机测试关闭 */
        if(strtg1547 && !testAlone)
        {
            Ieee1547_Run(0, IEEE_1547_THREAD_POLLING_INTERVAL * 1000000);
        }
        else
        {
            Ieee1547_Stop();
        }
    }
}


int Ieee1547_SendMessage(void)
{
    int err, msgId;
    char buf[MSG_DATA_LEN] = {0};

    if((err = MsgQueue_Get(MSG_QUEUE_PID_IEEE1547, &msgId)) < 0)
	{
        // PRINT_WARN("Get error[%d]", err);
		return -1;
	}

    if((err = MsgQueue_Send1(msgId, buf)) < 0)
    {
        PRINT_WARN("Send error[%d]", err);
        return -2;
    }

    return 0;
}


int Ieee1547_DeleteTimer(void)
{
    if(TimerInf_Delete(&timerIdIeee1547) != 0)
    {
        PRINT_WARN();
        return -1;
    }

    return 0;
}


int Ieee1547_GetTimerId(void)
{
    return (int)timerIdIeee1547;
}

